#include <stdio.h>
#include "string.h"
#include <esp_rom_sys.h>
#include "driver/spi_master.h"

#include "driver/gpio.h"
#include "esp_log.h"         
#include "RJMU401.h"
#include "freertos/task.h"

#define PIN_NUM_CS	    28
#define PIN_NUM_SIO		7
#define PIN_NUM_RST		3


#define PIN_NUM_MISO  13
#define PIN_NUM_MOSI  11
#define PIN_NUM_CLK   12

static spi_device_handle_t spi_handle;
/************************************************/

esp_err_t SPI_BUS_INIT()
{
    esp_err_t err;
    spi_bus_config_t bus_cfg = {
        .mosi_io_num = PIN_NUM_MOSI,
        .miso_io_num = PIN_NUM_MISO,
        .sclk_io_num = PIN_NUM_CLK,
        .quadwp_io_num = -1,
        .quadhd_io_num = -1,
        .max_transfer_sz = 4000,
    };

    err = spi_bus_initialize(SPI2_HOST, &bus_cfg, SPI_DMA_CH_AUTO);
    
    return err;
}

/************************************************/
esp_err_t RJMU_Crypto_Init()
{
    SPI_BUS_INIT();

    esp_err_t err;

    gpio_config_t io_conf = {
        .intr_type = GPIO_INTR_DISABLE,
        .mode = GPIO_MODE_INPUT,
        .pin_bit_mask = (1ULL<<PIN_NUM_SIO),
        .pull_up_en = GPIO_PULLUP_ENABLE
    };

    gpio_config(&io_conf);

    io_conf.mode = GPIO_MODE_OUTPUT;
    io_conf.pin_bit_mask = ((1ULL<<PIN_NUM_RST) | (1ULL<<PIN_NUM_CS));
    
    gpio_config(&io_conf);

	gpio_set_level(PIN_NUM_RST,1);
	esp_rom_delay_us(2000);
	gpio_set_level(PIN_NUM_RST,0);
	esp_rom_delay_us(10000);

    spi_device_interface_config_t devcfg={
        .clock_speed_hz=800*1000,           //Clock out at 10 MHz
        .mode=0,                            //SPI mode 0   <<<< CPOL    CPHA >>>>
        .spics_io_num =-1,                  //CS pin
        .queue_size   = 3,                  //We want to be able to queue 7 transactions at a time
    };

    gpio_set_level(PIN_NUM_CS , 0);
    err = spi_bus_add_device( SPI2_HOST , &devcfg, &spi_handle);


    return err;
}

/*********************************************************/
//                       CRC 32                          //
//描述：
//CRC32_ MPEG_2校验和计算软件实现
//参数：
//data: [输入]，计算 CRC32 的数据缓存地址指针
//length: [输入]，用于计算CRC32的数据长度
//r_buf: [输出]，按字节输出的CRC32校验和计算结果
//返回结果：
//按int型输出的CRC32校验和结
/*********************************************************/

int CRC32_MPEG_2(const uint8_t *data, int length, unsigned char crc_Buf[])
{
	unsigned char i;
	int CRC32 = 0xFFFFFFFF, j = 0;
	while ((length--) != 0)
	{
		CRC32 ^= (uint32_t)data[j] << 24;
		j++;
		for (i = 0; i < 8; ++i)
		{
			if ((CRC32 & 0x80000000) != 0)
				CRC32 = (CRC32 << 1) ^ 0x04C11DB7;
			else
				CRC32 <<= 1;
		}
	}
	
	//Reverse and wordtobyte
	for(i=0; i<4; i++)
	{
		crc_Buf[3-i] = (CRC32>>(8*i))&0xFF;		
	}
    return CRC32;
}


/***********************************************/
esp_err_t RJMU401_SendData( uint8_t *data , int n) {
    esp_err_t err;
    spi_transaction_t t = { .length = 8 , .flags = SPI_TRANS_USE_TXDATA };

    while (gpio_get_level(PIN_NUM_SIO) == 0) {vTaskDelay(100 / portTICK_PERIOD_MS); }

    esp_rom_delay_us(10);
    
    for (int i = 0 ; i < n ; i++) {
        t.tx_data[0] = data[i];
        err = spi_device_polling_transmit(spi_handle , &t);
        assert(err==ESP_OK);
        if (err != ESP_OK) return err;
        esp_rom_delay_us(10);
    }

    return ESP_OK;
}

/*************************************************** */

esp_err_t RJMU401_ReceiveData( uint8_t *data  , int n) {
    esp_err_t err;
    spi_transaction_t t = { .length = 8 , .flags = SPI_TRANS_USE_RXDATA };
    
    while (gpio_get_level(PIN_NUM_SIO) == 0) vTaskDelay(10 / portTICK_PERIOD_MS);
    
    esp_rom_delay_us(15);

    for ( int i = 0 ; i < n ; i++) {
        err = spi_device_polling_transmit(spi_handle , &t);
       
        if (err == ESP_OK) {
            data[i] = t.rx_data[0];
        } else return err;
    }
   
    return ESP_OK;
}

/*********************************************************/
//               Set the encrypt Key                    //
/*********************************************************/

esp_err_t Set_SM4_Key(uint8_t *Key ,size_t key_len) {
    esp_err_t err;
    int i;
    uint8_t m_stat[2]={0};
    uint8_t crc_Buf[4] = {0};

    uint8_t Key_crc[100] = {0};
  
    uint8_t Cmd[20] = {0xAA,0xC3,0x00,0xA2,0x00,0x14};
   
    CRC32_MPEG_2( Key, 16 , crc_Buf);

    for (i = 0; i < 20; i++) {
        if ( i < 16) {
            Key_crc[i] = Key[i];
        } else Key_crc[i] = crc_Buf[i - 16];
    }

    RJMU401_SendData( Cmd     ,  6 );
    err = RJMU401_ReceiveData( m_stat  , 2) ;
    if (err == ESP_OK) printf ("\nCmd Return:%2x %2x\n" , m_stat[0],m_stat[1]);


    RJMU401_SendData( Key_crc     , 20 );  
    err = RJMU401_ReceiveData( m_stat  , 2) ;
    if (err == ESP_OK) printf ("\nKey Return:%2x %2x\n" , m_stat[0],m_stat[1]);
    printf("wwww ");
    return ESP_OK;

}

/**************************************************************/
esp_err_t RJMU401_Get_ID() {
    esp_err_t err;
    uint8_t m_stat[100]={0} , cmd[6] = {0xAA,0xCE,0x01,0xA1,0x00,0x14};
   
    RJMU401_SendData( cmd     ,  6 );
    
    err = RJMU401_ReceiveData( m_stat  , 2) ;
    if (err == ESP_OK) printf ("\nGet ID Return:%2x %2x\n" , m_stat[0],m_stat[1]);

    err = RJMU401_ReceiveData( m_stat  , 0x14) ;


    if (err == ESP_OK) {


        for(int i = 0; i < 0x10; i++) {
            printf ("%2x " , m_stat[i]);
        }
    }
    return ESP_OK;

}

/**************************************************************/
esp_err_t RJMU401_encrypt( const uint8_t *plaintext, 
                           const uint8_t  plaintext_len ,
                           uint8_t  *ciphertest , 
                           uint16_t *ciphertest_len) 
{
    esp_err_t err;
    uint8_t Cmd[] = {0xAA, 0xC3, 0x00, 0xA1, 0x00, 0x80};
    uint8_t result_satu[2] , CRC_Buf[4] = {0};
    uint16_t Receive_len;

    CRC32_MPEG_2( plaintext , plaintext_len , CRC_Buf);

    Cmd[4] = ( ( plaintext_len + 4) & 0xFF00)>>8;  Cmd[5] = ( ( plaintext_len + 4) & 0xFF); 
    
    RJMU401_SendData( Cmd , 6 );
    err = RJMU401_ReceiveData( result_satu  , 2) ;

    if (err == ESP_OK) {
        if (result_satu[0] != 0x9a || result_satu[1] != 0x00) return ESP_FAIL;
    } else return ESP_FAIL;

    RJMU401_SendData( plaintext , plaintext_len );    RJMU401_SendData(  CRC_Buf , 4 );

    // 获取待接收数据长度
    err = RJMU401_ReceiveData( result_satu  , 2) ;

    if (err != ESP_OK) return ESP_FAIL;
    
    Receive_len = (((result_satu[0])&0xFF) << 8) + result_satu[1];

    printf("\n---%d---",Receive_len);

   

    if (*ciphertest_len < Receive_len) return ESP_FAIL;

    *ciphertest_len = 0;
    err = RJMU401_ReceiveData( ciphertest  , Receive_len) ;

    if (err == ESP_OK) *ciphertest_len = Receive_len ;
    printf("OK");
	return err;
}

/***************************************/

/**************************************************************/
esp_err_t RJMU401_decrypt( const uint8_t  *ciphertest , 
                           const uint16_t ciphertest_len ,
                           uint8_t  *plaintext , 
                           uint16_t  *plaintext_len ) 
{   esp_err_t err;
    uint8_t Cmd[] = {0xAA, 0xC3, 0x01, 0xA1, 0x00, 0x80};
    uint8_t result_satu[2] , CRC_Buf[4] = {0};
    uint16_t Receive_len;

    if ( ciphertest_len % 16 != 0) {
        *plaintext_len = 0;
        return ESP_FAIL;
    }

    CRC32_MPEG_2( ciphertest , ciphertest_len , CRC_Buf);

    Cmd[4] =  (( ciphertest_len + 4) & 0xFF00)>>8;  Cmd[5] = (ciphertest_len + 4) & 0xFF; 

    RJMU401_SendData( Cmd , 6 );
    err = RJMU401_ReceiveData( result_satu  , 2) ;

    if (err == ESP_OK) {
        if (result_satu[0] != 0x9a || result_satu[1] != 0x00) return ESP_FAIL;
    } else return ESP_FAIL;


    RJMU401_SendData( ciphertest , ciphertest_len );    RJMU401_SendData(  CRC_Buf , 4 );

    // 获取待接收数据长度
    err = RJMU401_ReceiveData( result_satu  , 2) ;

    if (err != ESP_OK) return ESP_FAIL;
    
    Receive_len = (((result_satu[0])&0xFF) << 8) + result_satu[1];

    printf("\n---%d---\n",Receive_len);

    if (*plaintext_len < Receive_len) return ESP_FAIL;

    *plaintext_len = 0;

    printf ("\n===================\n");
    
    err = RJMU401_ReceiveData( plaintext  , Receive_len) ;

    if (err == ESP_OK) *plaintext_len = Receive_len ;
    
	return err;
}